import serial
import time
import msvcrt

print "Started Simulation at :", time.clock()

Strgt 	= chr(0x32)
Left 	= chr(0x31)
Right	= chr(0x33)
Stop	= chr(0x34)
RotL 	= chr(0x35)

GPS_SPort 	= 'COM10' #GPS PORT
GpsPort 	= serial.Serial(GPS_SPort  , 9600, timeout=1)

Bot_SPort 	=  'COM12' #BOT PORT
BotPort 	= serial.Serial(Bot_SPort, 9600, timeout=1)

SampleGpsMsg = '#BESTUTMA,USB2,0,50.5,FINESTEERING,1692,265564.400,80000000,2775,6576;SOL_COMPUTED,SINGLE,17,R,3280538.0557,369509.1114,11.4042,0.0000,WGS84,1.8986,2.4845,3.1442,"",0.000,0.000,13,11,11,11,0,02,0,11*5009c08e\n'
GpsMsgArray  = SampleGpsMsg.split(',')
NewGpsMsg = ''

CurrentGPSx = GpsMsgArray[13]
CurrentGPSy = GpsMsgArray[14]
CurrentGPSz = GpsMsgArray[15]
CurrentTime = GpsMsgArray[6]

InputKey = 's'
deltax   = 22
deltay   = 22
delay    = 1
while InputKey != 'q':

	BotIn 			= BotPort.read()
	print BotIn
	CurrentTime 	= str(float(CurrentTime) + delay)
	GpsMsgArray[6]  = CurrentTime
	if( BotIn == Strgt):
		CurrentGPSx = str(float(CurrentGPSx) + deltax )
	if( BotIn == Left ):
		CurrentGPSx = str(float(CurrentGPSx) + deltax )
		CurrentGPSy = str(float(CurrentGPSy) + deltay )
	if( BotIn == Right):
		CurrentGPSx = str(float(CurrentGPSx) + deltax )
		CurrentGPSy = str(float(CurrentGPSy) - deltay )
	GpsMsgArray[13] = CurrentGPSx
	GpsMsgArray[14] = CurrentGPSy
	NewGpsMsg = ''
	for i in range(0,len(GpsMsgArray)):
		if i != 31:
			NewGpsMsg = NewGpsMsg + GpsMsgArray[i] +','
		else:
			NewGpsMsg = NewGpsMsg + GpsMsgArray[i]
	GpsPort.write(NewGpsMsg)		
	time.sleep(delay)
	print GPS_SPort+'> '+str(time.time())+'>'+NewGpsMsg

print "Exit Receiver"